// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcRK4Adapt.h"

//----------------------------------------------------------------------------
MgcRK4Adapt::MgcRK4Adapt (int iDim, MgcReal fStep, Function* aoF)
    :
    MgcODE(iDim,fStep,aoF)
{
    m_fStepUsed = 0.0;
    m_iPasses = 0;
    m_fSafety = 0.9;
    m_fEpsilon = 0.01;
    m_fCoeff = MgcMath::Pow(0.25*m_fSafety,0.2);

    m_afTemp1 = new MgcReal[m_iDim];
    m_afTemp2 = new MgcReal[m_iDim];
    m_afTemp3 = new MgcReal[m_iDim];
    m_afTemp4 = new MgcReal[m_iDim];
    m_afSave1 = new MgcReal[m_iDim];
    m_afXTemp  = new MgcReal[m_iDim];
    m_afXInter = new MgcReal[m_iDim];
    m_afXHalf  = new MgcReal[m_iDim];
    m_afXFull  = new MgcReal[m_iDim];
}
//----------------------------------------------------------------------------
MgcRK4Adapt::MgcRK4Adapt (int iDim, MgcReal fStep, AutoFunction* aoA)
    :
    MgcODE(iDim,fStep,aoA)
{
    m_fStepUsed = 0.0;
    m_iPasses = 0;
    m_fSafety = 0.9;
    m_fEpsilon = 0.01;
    m_fCoeff = MgcMath::Pow(0.25*m_fSafety,0.2);

    m_afTemp1 = new MgcReal[m_iDim];
    m_afTemp2 = new MgcReal[m_iDim];
    m_afTemp3 = new MgcReal[m_iDim];
    m_afTemp4 = new MgcReal[m_iDim];
    m_afSave1 = new MgcReal[m_iDim];
    m_afXTemp  = new MgcReal[m_iDim];
    m_afXInter = new MgcReal[m_iDim];
    m_afXHalf  = new MgcReal[m_iDim];
    m_afXFull  = new MgcReal[m_iDim];
}
//----------------------------------------------------------------------------
MgcRK4Adapt::~MgcRK4Adapt ()
{
    delete[] m_afTemp1;
    delete[] m_afTemp2;
    delete[] m_afTemp3;
    delete[] m_afTemp4;
    delete[] m_afSave1;
    delete[] m_afXTemp;
    delete[] m_afXInter;
    delete[] m_afXHalf;
    delete[] m_afXFull;
}
//----------------------------------------------------------------------------
void MgcRK4Adapt::SetSafety (MgcReal fSafety)
{
    m_fSafety = fSafety;
    m_fCoeff = MgcMath::Pow(0.25*m_fSafety,0.2);
}
//----------------------------------------------------------------------------
void MgcRK4Adapt::Update (MgcReal fTIn, MgcReal* afXIn, MgcReal& rfTOut,
    MgcReal* afXOut)
{
    m_iPasses = 1;
    while ( true )
    {
        MgcReal fH2 = 0.5*m_fStep;
        MgcReal fH4 = 0.25*m_fStep;
        MgcReal fH6 = m_fStep/6.0;
        MgcReal fH12 = m_fStep/12.0;
        MgcReal fT2 = fTIn + fH2;
        MgcReal fT4 = fTIn + fH4;
        MgcReal fT2pH4 = fT2 + fH4;
        rfTOut = fTIn + m_fStep;

        // take a half-step
        int i;
        for (i = 0; i < m_iDim; i++)  // RK4 first step
            m_afTemp1[i] = m_afSave1[i] = m_aoF[i](fTIn,afXIn);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+fH4*m_afTemp1[i];
        for (i = 0; i < m_iDim; i++)  // RK4 second step
            m_afTemp2[i] = m_aoF[i](fT4,m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+fH4*m_afTemp2[i];
        for (i = 0; i < m_iDim; i++)  // RK4 third step
            m_afTemp3[i] = m_aoF[i](fT4,m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+fH2*m_afTemp3[i];
        for (i = 0; i < m_iDim; i++)  // RK4 fourth step
            m_afTemp4[i] = m_aoF[i](fT2,m_afXTemp);
        for (i = 0; i < m_iDim; i++)
        {
            m_afXInter[i] = afXIn[i]+fH12*(m_afTemp1[i]+
                2.0*(m_afTemp2[i]+m_afTemp3[i])+m_afTemp4[i]);
        }

        // take another half-step
        for (i = 0; i < m_iDim; i++)  // RK4 first step
            m_afTemp1[i] = m_aoF[i](fT2,m_afXInter);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = m_afXInter[i]+fH4*m_afTemp1[i];
        for (i = 0; i < m_iDim; i++)  // RK4 second step
            m_afTemp2[i] = m_aoF[i](fT2pH4,m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = m_afXInter[i]+fH4*m_afTemp2[i];
        for (i = 0; i < m_iDim; i++)  // RK4 third step
            m_afTemp3[i] = m_aoF[i](fT2pH4,m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = m_afXInter[i]+fH2*m_afTemp3[i];
        for (i = 0; i < m_iDim; i++)  // RK4 fourth step
            m_afTemp4[i] = m_aoF[i](rfTOut,m_afXTemp);
        for (i = 0; i < m_iDim; i++)
        {
            m_afXHalf[i] = m_afXInter[i]+fH12*(m_afTemp1[i]+
                2.0*(m_afTemp2[i]+m_afTemp3[i])+m_afTemp4[i]);
        }

        // take a full-step
        for (i = 0; i < m_iDim; i++)  // RK4 first step
            m_afXTemp[i] = afXIn[i]+fH2*m_afSave1[i];
        for (i = 0; i < m_iDim; i++)  // RK4 second step
            m_afTemp2[i] = m_aoF[i](fT2,m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+fH2*m_afTemp2[i];
        for (i = 0; i < m_iDim; i++)  // RK4 third step
            m_afTemp3[i] = m_aoF[i](fT2,m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+m_fStep*m_afTemp3[i];
        for (i = 0; i < m_iDim; i++)  // RK4 fourth step
            m_afTemp4[i] = m_aoF[i](rfTOut,m_afXTemp);
        for (i = 0; i < m_iDim; i++)
        {
            m_afXFull[i] = afXIn[i]+fH6*(m_afSave1[i]+
                2.0*(m_afTemp2[i]+m_afTemp3[i])+m_afTemp4[i]);
        }

        // compute error term
        MgcReal fMaxRatio = 0.0;
        for (i = 0; i < m_iDim; i++)
        {
            m_afXTemp[i] = m_afXHalf[i]-m_afXFull[i];
            MgcReal fRatio = MgcMath::Abs(m_afXTemp[i]/(m_fStep*m_afSave1[i]
                +1e-16));
            if ( fMaxRatio < fRatio )
                fMaxRatio = fRatio;
        }
        fMaxRatio /= m_fEpsilon;

        if ( fMaxRatio <= 1.0 )
        {
            // increase the step size
            m_fStepUsed = m_fStep;
            if ( fMaxRatio > m_fCoeff )
                m_fStep *= m_fSafety/MgcMath::Pow(fMaxRatio,0.2);
            else
                m_fStep *= 4.0;
            break;
        }
        // else decrease the step size, recompute solution with it
        m_iPasses++;
        m_fStep *= m_fSafety/MgcMath::Pow(fMaxRatio,0.25);
        if ( m_fStep == 0.0 )  // step size became too small
            return;
    }

    // generate output
    for (int i = 0; i < m_iDim; i++)
        afXOut[i] = m_afXHalf[i] + m_afXTemp[i]/15.0;
}
//----------------------------------------------------------------------------
void MgcRK4Adapt::Update (MgcReal* afXIn, MgcReal* afXOut)
{
    m_iPasses = 1;
    while ( true )
    {
        MgcReal fH2 = m_fStep/2;
        MgcReal fH4 = m_fStep/4;
        MgcReal fH6 = m_fStep/6;
        MgcReal fH12 = m_fStep/12;

        // take a half-step
        int i;
        for (i = 0; i < m_iDim; i++)  // RK4 first step
            m_afTemp1[i] = m_afSave1[i] = m_aoA[i](afXIn);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+fH4*m_afTemp1[i];
        for (i = 0; i < m_iDim; i++)  // RK4 second step
            m_afTemp2[i] = m_aoA[i](m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+fH4*m_afTemp2[i];
        for (i = 0; i < m_iDim; i++)  // RK4 third step
            m_afTemp3[i] = m_aoA[i](m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+fH2*m_afTemp3[i];
        for (i = 0; i < m_iDim; i++)  // RK4 fourth step
            m_afTemp4[i] = m_aoA[i](m_afXTemp);
        for (i = 0; i < m_iDim; i++)
        {
            m_afXInter[i] = afXIn[i]+fH12*(m_afTemp1[i]+
                2.0*(m_afTemp2[i]+m_afTemp3[i])+m_afTemp4[i]);
        }

        // Take another half-step
        for (i = 0; i < m_iDim; i++)  // RK4 first step
            m_afTemp1[i] = m_aoA[i](m_afXInter);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = m_afXInter[i]+fH4*m_afTemp1[i];
        for (i = 0; i < m_iDim; i++)  // RK4 second step
            m_afTemp2[i] = m_aoA[i](m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = m_afXInter[i]+fH4*m_afTemp2[i];
        for (i = 0; i < m_iDim; i++)  // RK4 third step
            m_afTemp3[i] = m_aoA[i](m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = m_afXInter[i]+fH2*m_afTemp3[i];
        for (i = 0; i < m_iDim; i++)  // RK4 fourth step
            m_afTemp4[i] = m_aoA[i](m_afXTemp);
        for (i = 0; i < m_iDim; i++)
        {
            m_afXHalf[i] = m_afXInter[i]+fH12*(m_afTemp1[i]+
                2.0*(m_afTemp2[i]+m_afTemp3[i])+m_afTemp4[i]);
        }

        // Take a full-step
        for (i = 0; i < m_iDim; i++)  // RK4 first step
            m_afXTemp[i] = afXIn[i]+fH2*m_afSave1[i];
        for (i = 0; i < m_iDim; i++)  // RK4 second step
            m_afTemp2[i] = m_aoA[i](m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+fH2*m_afTemp2[i];
        for (i = 0; i < m_iDim; i++)  // RK4 third step
            m_afTemp3[i] = m_aoA[i](m_afXTemp);
        for (i = 0; i < m_iDim; i++)
            m_afXTemp[i] = afXIn[i]+m_fStep*m_afTemp3[i];
        for (i = 0; i < m_iDim; i++)  // RK4 fourth step
            m_afTemp4[i] = m_aoA[i](m_afXTemp);
        for (i = 0; i < m_iDim; i++)
        {
            m_afXFull[i] = afXIn[i]+fH6*(m_afSave1[i]+
                2.0*(m_afTemp2[i]+m_afTemp3[i])+m_afTemp4[i]);
        }

        // compute error term
        MgcReal fMaxRatio = 0.0; 
        for (i = 0; i < m_iDim; i++)
        {
            m_afXTemp[i] = m_afXHalf[i]-m_afXFull[i];
            MgcReal fRatio = MgcMath::Abs(m_afXTemp[i]/(m_fStep*m_afSave1[i]
                +1e-16));
            if ( fMaxRatio < fRatio )
                fMaxRatio = fRatio;
        }
        fMaxRatio /= m_fEpsilon;

        if ( fMaxRatio <= 1.0 )
        {
            // increase the step size
            m_fStepUsed = m_fStep;
            if ( fMaxRatio > m_fCoeff )
                m_fStep *= m_fSafety/MgcMath::Pow(fMaxRatio,0.2);
            else
                m_fStep *= 4.0;
            break;
        }
        // else decrease the step size, recompute solution with it
        m_iPasses++;
        m_fStep *= m_fSafety/MgcMath::Pow(fMaxRatio,0.25);
        if ( m_fStep == 0.0 )  // step size became too small
            return;
    }

    // generate output
    for (int i = 0; i < m_iDim; i++)
        afXOut[i] = m_afXHalf[i] + m_afXTemp[i]/15.0;
}
//----------------------------------------------------------------------------
void MgcRK4Adapt::SetStepSize (MgcReal fStep)
{
    m_fStep = fStep;
    m_fStepUsed = 0.0;
}
//----------------------------------------------------------------------------

#ifdef RK4ADAPT_TEST

#include "iostream.h"

// x'(t) = 1, y'(t) = 2y, x(0) = -1, y(0) = 1
// solution is x(t) = t-1, y(t) = exp(2t), so y = exp(2(x+1))
MgcReal F0 (MgcReal fT, MgcReal* afX) { return 1.0; }
MgcReal F1 (MgcReal fT, MgcReal* afX) { return 2*afX[1]; }

int main ()
{
    const int iDim = 2;
    const MgcReal fStep = 0.01;
    MgcODE::Function aoF[2] = { F0, F1 };

    MgcRK4Adapt kODE(iDim,fStep,aoF);
    MgcReal fTIn = 0.0, fTOut;
    MgcReal afXIn[iDim] = { -1.0, 1.0 }, afXOut[iDim];

    cout << "t x y step_tried step_used passes" << endl;
    for (int i = 0; i < 10; i++)
    {
        cout << fTIn << ' ' << afXIn[0] << ' ' << afXIn[1] << ' ';
        cout << kODE.GetStepSize() << ' ' << kODE.GetStepUsed() << ' ';
        cout << kODE.GetPasses() << endl;
        kODE.Update(fTIn,afXIn,fTOut,afXOut);
        for (int j = 0; j < iDim; j++)
            afXIn[j] = afXOut[j];
        fTIn = fTOut;
    }

    // t          x           y       step_tried  step_used  passes
    // 0         -1           1       0.01        0          0
    // 0.01      -0.99        1.0202  0.04        0.01       1
    // 0.05      -0.95        1.10517 0.16        0.04       1
    // 0.21      -0.79        1.52196 0.373121    0.16       1
    // 0.583121  -0.416879    3.20983 0.436545    0.373121   1
    // 1.01967    0.0196654   7.68482 0.448811    0.436545   1
    // 1.46848    0.468476   18.8554  0.450984    0.448811   1
    // 1.91946    0.91946    46.4649  0.45136     0.450984   1
    // 2.37082    1.37082   114.588   0.451424    0.45136    1
    // 2.82224    1.82224   282.626   0.451437    0.451424   1

    return 0;
}

#endif
